﻿// -------------------------------------------------------------------------------------------------------------------------------
// <copyright file="GmresSolver.cs" company="Exul">
//     Copyright (c) Exul. All rights reserved.
// </copyright>
// <author>
//     Exul
// </author>
// -------------------------------------------------------------------------------------------------------------------------------

namespace ExulLibrary.Mathematics.LinearAlgebra.Sequential.DecimalExtensions
{
    #region Usings

    using System;
    using System.Collections.Generic;
    using System.Diagnostics.CodeAnalysis;

    using ExulLibrary.Exceptions;
    using ExulLibrary.Mathematics.LinearAlgebra.Exceptions;

    #endregion

    /// <summary>
    ///     Represents an iterative solver of a system of linear algebraic equations that uses GMRES-algorithm.
    /// </summary>
    public sealed class GmresSolver : IterativeSlaeSolver<decimal>
    {
        #region Constants

        #region public

        /// <summary>
        ///     The name of spatial dimension parameter.
        /// </summary>
        public const string SpatialDimensionParameterName = "spatialdimension";

        #endregion

        #region private

        /// <summary>
        ///     The minimal acceptable under diagonal element.
        /// </summary>
        private const decimal MinimalUnderDiagonalElement = 1e-26m;

        #endregion

        #endregion

        #region Fields

        #region private

        /// <summary>
        ///     The matrix H.
        /// </summary>
        private DenseVector<decimal>[] matrixH;

        /// <summary>
        ///     The matrix V.
        /// </summary>
        private DenseVector<decimal>[] matrixV;

        /// <summary>
        ///     The vector D.
        /// </summary>
        private DenseVector<decimal> vectorD;

        /// <summary>
        ///     The vector M^-1*v_i.
        /// </summary>
        private DenseVector<decimal> vectorMV;

        /// <summary>
        ///     The residual vector.
        /// </summary>
        private DenseVector<decimal> vectorR;

        /// <summary>
        ///     The vector W.
        /// </summary>
        private DenseVector<decimal> vectorW;

        /// <summary>
        ///     The vector Z.
        /// </summary>
        private DenseVector<decimal> vectorZ;

        #endregion

        #endregion

        #region Properties

        #region public

        /// <summary>
        ///     Gets the spatial dimension.
        /// </summary>
        /// <value>
        ///     The spatial dimension.
        /// </value>
        public int SpatialDimension
        {
            get;
            private set;
        }

        #endregion

        #endregion

        #region Overridden Methods

        #region public

        /// <summary>
        ///     Initializes the solver.
        /// </summary>
        /// <param name="targetSize">
        ///     The size of the target system to solve.
        /// </param>
        /// <param name="stopCriterion">
        ///     The stop criterion.
        /// </param>
        /// <param name="parameters">
        ///     The key-value collection of additional parameters for the solver.
        /// </param>
        /// <exception cref="ArgumentNullException">
        ///     <paramref name="stopCriterion"/> is <see langword="null"/>.
        /// </exception>
        /// <exception cref="NegativeOrZeroSizeException">
        ///     <paramref name="targetSize"/> is negative or equal to zero.
        /// </exception>
        /// <exception cref="NotFoundSolverParameterException">
        ///     <paramref name="parameters"/> doesn't contain all required parameters.
        /// </exception>
        public override void Initialize(
            int targetSize, IterationsStopCriterion<decimal> stopCriterion, Dictionary<string, object> parameters)
        {
#if SAFE

            if ((parameters == null) ||
                !parameters.ContainsKey(SpatialDimensionParameterName))
            {
                throw new NotFoundSolverParameterException("parameters", SpatialDimensionParameterName);
            }

            if (!(parameters[SpatialDimensionParameterName] is int))
            {
                throw new SolverParameterTypeException(
                    "parameters",
                    SpatialDimensionParameterName,
                    parameters[SpatialDimensionParameterName].GetType(),
                    typeof(int));
            }

#endif

            base.Initialize(targetSize, stopCriterion, parameters);

            this.SpatialDimension = (int)parameters[SpatialDimensionParameterName];

            this.vectorR = new DenseVector<decimal>(targetSize);
            this.vectorW = new DenseVector<decimal>(targetSize);
            this.vectorMV = new DenseVector<decimal>(targetSize);
            this.vectorD = new DenseVector<decimal>(this.SpatialDimension + 1);
            this.vectorZ = new DenseVector<decimal>(this.SpatialDimension);
            this.matrixV = new DenseVector<decimal>[this.SpatialDimension];
            this.matrixH = new DenseVector<decimal>[this.SpatialDimension];
            for (var index = 0; index < this.SpatialDimension; index++)
            {
                this.matrixV[index] = new DenseVector<decimal>(targetSize);
                this.matrixH[index] = new DenseVector<decimal>(index + 2);
            }
        }

        #endregion

        #region protected

        /// <summary>
        ///     Solves the specified system without preconditioning.
        /// </summary>
        /// <param name="system">
        ///     The system to solve.
        /// </param>
        /// <param name="solution">
        ///     The vector to store the solution.
        /// </param>
        /// <param name="initialSolution">
        ///     The initial solution.
        /// </param>
        [SuppressMessage(
            "Microsoft.Design",
            "CA1062:Validate arguments of public methods",
            MessageId = "0",
            Justification = "All parameters are verified in the caller method.")]
        protected override void SolveSystem(
            Slae<decimal> system, DenseVector<decimal> solution, DenseVector<decimal> initialSolution)
        {
            if (initialSolution != null)
            {
                solution.Assign(initialSolution);
            }
            else
            {
                solution.Clear();
            }

            // A * x_0
            system.Matrix.Multiply(solution, this.vectorMV);
            this.vectorR.Assign(system.RightHandPart);

            // R = b - A * x_0
            this.vectorR.Subtract(this.vectorMV);

            // r_0 = ||R||
            var initialResidual = (decimal)Math.Sqrt((double)this.vectorR.DotProduct());
            var residual = initialResidual;
            var iterationData = new IterationData<decimal>
                                {
                                    IterationIndex = 0,
                                    Residual = residual / initialResidual
                                };

            while (!this.StopCriterion.ShouldStop(iterationData))
            {
                // V_0 = R / ||R||
                this.matrixV[0].Assign(this.vectorR);
                this.matrixV[0].Multiply(1.0m / residual);

                var mu = 0;
                while (mu < this.SpatialDimension)
                {
                    // W = A*V_mu
                    system.Matrix.Multiply(this.matrixV[mu], this.vectorW);

                    var columnH = this.matrixH[mu].Values;
                    for (var lambda = 0; lambda <= mu; lambda++)
                    {
                        // H_lambda_mu = (W,V_lambda)
                        columnH[lambda] = this.vectorW.DotProduct(this.matrixV[lambda]);

                        // W -= H_lambda_mu * V_lambda
                        this.vectorW.Subtract(this.matrixV[lambda], columnH[lambda]);
                    }

                    mu++;

                    // H_mu+1_mu = ||W||
                    columnH[mu] = (decimal)Math.Sqrt((double)this.vectorW.DotProduct());
                    if (columnH[mu] < MinimalUnderDiagonalElement)
                    {
                        break;
                    }

                    if (mu < this.SpatialDimension)
                    {
                        // V_mu+1 = W / ||W||
                        this.matrixV[mu].Assign(this.vectorW);
                        this.matrixV[mu].Multiply(1.0m / columnH[mu]);
                    }
                }

                // Given's rotation
                // D_0 = ||R||
                this.vectorD.Values[0] = residual;
                for (var rowIndex = 0; rowIndex < mu; rowIndex++)
                {
                    var diagonalElement = this.matrixH[rowIndex].Values[rowIndex];
                    var underDiagonalElement = this.matrixH[rowIndex].Values[rowIndex + 1];
                    var divisor = (decimal)Math.Sqrt(
                        (double)((diagonalElement * diagonalElement) + (underDiagonalElement * underDiagonalElement)));

                    // C = H_i_i / sqrt(H_i_i*H_i_i + H_i+1_i*H_i+1_i)
                    var factorC = diagonalElement / divisor;

                    // S = H_i+1_i / sqrt(H_i_i*H_i_i + H_i+1_i*H_i+1_i)
                    var factorS = underDiagonalElement / divisor;
                    for (var columnIndex = rowIndex; columnIndex < mu; columnIndex++)
                    {
                        var highElement = this.matrixH[columnIndex].Values[rowIndex];
                        var lowElement = this.matrixH[columnIndex].Values[rowIndex + 1];

                        // H_i_j = C * H_i_j + S * H_i+1_j
                        this.matrixH[columnIndex].Values[rowIndex] = (factorC * highElement) + (factorS * lowElement);

                        // H_i+1_j = C * H_i+1_j - S * H_i_j
                        this.matrixH[columnIndex].Values[rowIndex + 1] = (factorC * lowElement) - (factorS * highElement);
                    }

                    var vectorHighElement = this.vectorD.Values[rowIndex];

                    // D_i = C * D_i
                    this.vectorD.Values[rowIndex] = factorC * vectorHighElement;

                    // D_i+1 = -S * D_i
                    this.vectorD.Values[rowIndex + 1] = -factorS * vectorHighElement;
                }

                // solving triangular SLAE
                this.vectorZ.Clear();
                for (var columnIndex = mu - 1; 0 <= columnIndex; columnIndex--)
                {
                    var columnH = this.matrixH[columnIndex].Values;
                    var multiplierElement = this.vectorD.Values[columnIndex] / columnH[columnIndex];
                    this.vectorZ.Values[columnIndex] = multiplierElement;
                    for (var rowIndex = 0; rowIndex < columnIndex; rowIndex++)
                    {
                        this.vectorD.Values[rowIndex] -= columnH[rowIndex] * multiplierElement;
                    }
                }

                // X_k = X_k-1 + V*Z
                this.vectorW.Clear();
                for (var columnIndex = 0; columnIndex < mu; columnIndex++)
                {
                    var columnV = this.matrixV[columnIndex].Values;
                    var multiplierElement = this.vectorZ.Values[columnIndex];
                    for (var rowIndex = 0; rowIndex < this.TargetSize; rowIndex++)
                    {
                        this.vectorW.Values[rowIndex] += multiplierElement * columnV[rowIndex];
                    }
                }

                // x = x + V*Z
                solution.Add(this.vectorW);

                // R = b - A * X_k
                system.Matrix.Multiply(solution, this.vectorMV);
                this.vectorR.Assign(system.RightHandPart);
                this.vectorR.Subtract(this.vectorMV);

                // r = ||R||
                residual = (decimal)Math.Sqrt((double)this.vectorR.DotProduct());

                iterationData.IterationIndex++;
                iterationData.Residual = residual / initialResidual;
            }
        }

        /// <summary>
        ///     Solves the specified system with preconditioning.
        /// </summary>
        /// <param name="system">
        ///     The system to solve.
        /// </param>
        /// <param name="solution">
        ///     The vector to store the solution.
        /// </param>
        /// <param name="preconditioningMatrix">
        ///     The preconditioning matrix.
        /// </param>
        /// <param name="initialSolution">
        ///     The initial solution.
        /// </param>
        [SuppressMessage(
            "Microsoft.Design",
            "CA1062:Validate arguments of public methods",
            MessageId = "0",
            Justification = "All parameters are verified in the caller method.")]
        protected override void SolveSystem(
            Slae<decimal> system,
            DenseVector<decimal> solution,
            DecompositionMatrix<decimal> preconditioningMatrix,
            DenseVector<decimal> initialSolution)
        {
            if (initialSolution != null)
            {
                solution.Assign(initialSolution);
            }
            else
            {
                solution.Clear();
            }

            // A * x_0
            system.Matrix.Multiply(solution, this.vectorMV);
            this.vectorR.Assign(system.RightHandPart);

            // R = b - A * x_0
            this.vectorR.Subtract(this.vectorMV);

            // r_0 = ||R||
            var initialResidual = (decimal)Math.Sqrt((double)this.vectorR.DotProduct());
            var residual = initialResidual;
            var iterationData = new IterationData<decimal>
                                {
                                    IterationIndex = 0,
                                    Residual = residual / initialResidual
                                };

            while (!this.StopCriterion.ShouldStop(iterationData))
            {
                // V_0 = R / ||R||
                this.matrixV[0].Assign(this.vectorR);
                this.matrixV[0].Multiply(1.0m / residual);

                var mu = 0;
                while (mu < this.SpatialDimension)
                {
                    // MV = M^-1 * V
                    this.vectorMV.Assign(this.matrixV[mu]);
                    preconditioningMatrix.Solve(this.vectorMV);

                    // W = A*M^1*V_mu
                    system.Matrix.Multiply(this.vectorMV, this.vectorW);

                    var columnH = this.matrixH[mu].Values;
                    for (var lambda = 0; lambda <= mu; lambda++)
                    {
                        // H_lambda_mu = (W,V_lambda)
                        columnH[lambda] = this.vectorW.DotProduct(this.matrixV[lambda]);

                        // W -= H_lambda_mu * V_lambda
                        this.vectorW.Subtract(this.matrixV[lambda], columnH[lambda]);
                    }

                    mu++;

                    // H_mu+1_mu = ||W||
                    columnH[mu] = (decimal)Math.Sqrt((double)this.vectorW.DotProduct());
                    if (columnH[mu] < MinimalUnderDiagonalElement)
                    {
                        break;
                    }

                    if (mu < this.SpatialDimension)
                    {
                        // V_mu+1 = W / ||W||
                        this.matrixV[mu].Assign(this.vectorW);
                        this.matrixV[mu].Multiply(1.0m / columnH[mu]);
                    }
                }

                // Given's rotation
                // D_0 = ||R||
                this.vectorD.Values[0] = residual;
                for (var rowIndex = 0; rowIndex < mu; rowIndex++)
                {
                    var diagonalElement = this.matrixH[rowIndex].Values[rowIndex];
                    var underDiagonalElement = this.matrixH[rowIndex].Values[rowIndex + 1];
                    var divisor = (decimal)Math.Sqrt(
                        (double)((diagonalElement * diagonalElement) + (underDiagonalElement * underDiagonalElement)));

                    // C = H_i_i / sqrt(H_i_i*H_i_i + H_i+1_i*H_i+1_i)
                    var factorC = diagonalElement / divisor;

                    // S = H_i+1_i / sqrt(H_i_i*H_i_i + H_i+1_i*H_i+1_i)
                    var factorS = underDiagonalElement / divisor;
                    for (var columnIndex = rowIndex; columnIndex < mu; columnIndex++)
                    {
                        var highElement = this.matrixH[columnIndex].Values[rowIndex];
                        var lowElement = this.matrixH[columnIndex].Values[rowIndex + 1];

                        // H_i_j = C * H_i_j + S * H_i+1_j
                        this.matrixH[columnIndex].Values[rowIndex] = (factorC * highElement) + (factorS * lowElement);

                        // H_i+1_j = C * H_i+1_j - S * H_i_j
                        this.matrixH[columnIndex].Values[rowIndex + 1] = (factorC * lowElement) - (factorS * highElement);
                    }

                    var vectorHighElement = this.vectorD.Values[rowIndex];

                    // D_i = C * D_i
                    this.vectorD.Values[rowIndex] = factorC * vectorHighElement;

                    // D_i+1 = -S * D_i
                    this.vectorD.Values[rowIndex + 1] = -factorS * vectorHighElement;
                }

                // solving triangular SLAE
                this.vectorZ.Clear();
                for (var columnIndex = mu - 1; 0 <= columnIndex; columnIndex--)
                {
                    var columnH = this.matrixH[columnIndex].Values;
                    var multiplierElement = this.vectorD.Values[columnIndex] / columnH[columnIndex];
                    this.vectorZ.Values[columnIndex] = multiplierElement;
                    for (var rowIndex = 0; rowIndex < columnIndex; rowIndex++)
                    {
                        this.vectorD.Values[rowIndex] -= columnH[rowIndex] * multiplierElement;
                    }
                }

                // X_k = X_k-1 + M^-1*V*Z
                this.vectorW.Clear();
                for (var columnIndex = 0; columnIndex < mu; columnIndex++)
                {
                    var columnV = this.matrixV[columnIndex].Values;
                    var multiplierElement = this.vectorZ.Values[columnIndex];
                    for (var rowIndex = 0; rowIndex < this.TargetSize; rowIndex++)
                    {
                        this.vectorW.Values[rowIndex] += multiplierElement * columnV[rowIndex];
                    }
                }

                // W=M^-1*V
                this.vectorMV.Assign(this.vectorW);
                preconditioningMatrix.Solve(this.vectorMV);

                // x = x + M^-1*V
                solution.Add(this.vectorMV);

                // R = b - A * X_k
                system.Matrix.Multiply(solution, this.vectorMV);
                this.vectorR.Assign(system.RightHandPart);
                this.vectorR.Subtract(this.vectorMV);

                // r = ||R||
                residual = (decimal)Math.Sqrt((double)this.vectorR.DotProduct());

                iterationData.IterationIndex++;
                iterationData.Residual = residual / initialResidual;
            }
        }

        #endregion

        #endregion
    }
}